﻿#include "FilterPassType.h"
#include "IIRUtil.h"
#include "array/Array.hpp"
#include "array/Arrays.h"
#include "dsp/BiquadIIR.h"
#include "dsp/IIR.h"
#include "lang/Exception.h"
#include "util/Util.h"
#include "yzrutil.h"
#include <cmath>
#include <cstdint>
#include <memory>
#include <vector>

using namespace yzrilyzr_util;
using namespace yzrilyzr_array;
using namespace yzrilyzr_lang;

namespace yzrilyzr_dsp{
	std::shared_ptr<IIR> IIRUtil::newButterworthIIRFilter(u_sample_rate sampleRate, FilterPassType type, uint16_t n, u_freq f1, u_freq f2){
		return newButterworthIIRFilter(type, n, f1 / sampleRate, f2 / sampleRate);
	}
	/**
	 * @param n  阶数
	 * @param f1 数字角频率，起始
	 * @param f2 数字角频率，结束（高通低通填0即可）
	 */
	std::shared_ptr<IIR> IIRUtil::newButterworthIIRFilter(FilterPassType type, uint16_t n, u_freq f1, u_freq f2){
		return std::make_shared<IIR>(design(type, n, f1, f2));
	}
	std::shared_ptr<yzrilyzr_array::DoubleArray> * IIRUtil::design(FilterPassType filterPassType, uint16_t filterOrder, u_freq fcf1, u_freq fcf2){
		if(filterOrder < 1){
			throw Exception("Invalid filterOrder.");
		}
		if(fcf1 <= 0 || fcf1 >= 0.5){
			throw Exception("Invalid fcf1.");
		}
		if(filterPassType == FilterPassType::BANDPASS || filterPassType == FilterPassType::BANDSTOP){
			if(fcf2 <= 0 || fcf2 >= 0.5){
				throw Exception("Invalid fcf2.");
			}
		}
		std::shared_ptr<yzrilyzr_array::DoubleArray> * coeffs=new std::shared_ptr<yzrilyzr_array::DoubleArray>[2];
		coeffs[0]=calculateACoefficients(filterPassType, filterOrder, fcf1, fcf2);
		coeffs[1]=calculateBCoefficients(filterPassType, filterOrder, fcf1, fcf2);
		double scalingFactor=calculateScalingFactor(filterPassType, filterOrder, fcf1, fcf2);
		if(isnan(scalingFactor)) throw Exception("NaN");
		DoubleArray & co1=*coeffs[1];
		for(u_index i=0, j=co1.length;i < j;i++) co1[i]*=scalingFactor;
		return coeffs;
	}
	//--- A coefficients -----------------------------------------------------------
	std::shared_ptr<DoubleArray> IIRUtil::calculateACoefficients(FilterPassType filterPassType, uint16_t filterOrder, u_freq fcf1, u_freq fcf2){
		switch(filterPassType){
			case LOWPASS:
			case HIGHPASS:
				return calculateACoefficients_lp_hp(filterOrder, fcf1);
			case BANDPASS:
			case BANDSTOP:
				return calculateACoefficients_bp_bs(filterPassType, filterOrder, fcf1, fcf2);
			default:
				throw Exception();
		}
	}
	//--- B coefficients -----------------------------------------------------------
	// Calculates the B coefficients.
	std::shared_ptr<DoubleArray> IIRUtil::calculateBCoefficients(FilterPassType filterPassType, uint16_t filterOrder, u_freq fcf1, u_freq fcf2){
		switch(filterPassType){
			case LOWPASS:
			{
				auto a=calculateBCoefficients_lp(filterOrder);
				return Arrays::cast<double>(a);
			}
			case HIGHPASS:
			{
				auto a=calculateBCoefficients_hp(filterOrder);
				return Arrays::cast<double>(a);
			}
			case BANDPASS:
			{
				auto a=calculateBCoefficients_bp(filterOrder);
				return Arrays::cast<double>(a);
			}
			case BANDSTOP:
			{
				return calculateBCoefficients_bs(filterOrder, fcf1, fcf2);
			}
			default:
				throw Exception();
		}
	}
	//--- Scaling factor -----------------------------------------------------------
	// Calculates the scaling factor.
	// The scaling factor is what the B coefficients must be multiplied by so
	// that the filter response has a maximum value of 1.
	double IIRUtil::calculateScalingFactor(FilterPassType filterPassType, uint16_t filterOrder, u_freq fcf1, u_freq fcf2){
		switch(filterPassType){
			case LOWPASS:
			case HIGHPASS:
				return calculateScalingFactor_lp_hp(filterPassType, filterOrder, fcf1);
			case BANDPASS:
			case BANDSTOP:
				return calculateScalingFactor_bp_bs(filterPassType, filterOrder, fcf1, fcf2);
			default:
				throw Exception();
		}
	}
	// Calculates the A coefficients for a lowpass or highpass filter.
	std::shared_ptr<DoubleArray> IIRUtil::calculateACoefficients_lp_hp(uint16_t n, u_freq fcf){
		DoubleArray rcof(2 * (u_index)n);                      // binomial coefficients
		double theta=2 * Math::PI * fcf;
		double st=sin(theta);
		double ct=cos(theta);
		for(u_index k=0;k < n;k++){
			double parg=Math::PI * (2 * k + 1) / (2 * n);       // pole angle
			double sparg=sin(parg);
			double cparg=cos(parg);
			double a=1 + st * sparg;
			rcof[2 * k]=-ct / a;
			rcof[2 * k + 1]=-st * cparg / a;
		}
		auto wcof1=binomial_mult(n, rcof);
		auto & wcof=*wcof1;
		auto dcof1=std::make_shared<DoubleArray>(n);
		DoubleArray & dcof=*dcof1;
		for(u_index k=0;k < n;k++){
			dcof[k]=wcof[2 * k];
		}
		return dcof1;
	}
	// Calculates the A coefficients for a bandpass or bandstop filter.
	std::shared_ptr<DoubleArray> IIRUtil::calculateACoefficients_bp_bs(FilterPassType filterPassType, uint16_t n, u_freq f1f, u_freq f2f){
		double cp=cos(2 * Math::PI * (f2f + f1f) / 2);
		double theta=2 * Math::PI * (f2f - f1f) / 2;
		double st=sin(theta);
		double ct=cos(theta);
		double s2t=2 * st * ct;                               // sine of 2*theta
		double c2t=2 * ct * ct - 1;                           // cosine of 2*theta
		DoubleArray rcof(2 * n);                      // z^-2 coefficients
		DoubleArray tcof(2 * n);                      // z^-1 coefficients
		double flip=(filterPassType == FilterPassType::BANDSTOP)?-1:1;
		for(u_index k=0;k < n;k++){
			double parg=Math::PI * (2 * k + 1) / (2 * n);       // pole angle
			double sparg=sin(parg);
			double cparg=cos(parg);
			double a=1 + s2t * sparg;
			rcof[2 * k]=c2t / a;
			rcof[2 * k + 1]=s2t * cparg / a * flip;
			tcof[2 * k]=-2 * cp * (ct + st * sparg) / a;
			tcof[2 * k + 1]=-2 * cp * st * cparg / a * flip;
		}
		auto wcof1=trinomial_mult(n, tcof, rcof);
		DoubleArray & wcof=*wcof1;
		auto dcof1=std::make_shared<DoubleArray>(2 * n);
		DoubleArray & dcof=*dcof1;
		for(u_index k=0;k < 2 * n;k++){
			dcof[k]=wcof[2 * k];
		}
		return dcof1;
	}
	// Calculates the B coefficients for a lowpass filter.
	std::shared_ptr<IntArray> IIRUtil::calculateBCoefficients_lp(uint16_t n){
		auto ccof1=std::make_shared<IntArray>(n + 1);
		IntArray & ccof=*ccof1;
		ccof[0]=1;
		ccof[1]=n;
		int m=n / 2;
		for(u_index i=2;i <= m;i++){
			ccof[i]=(n - i + 1) * ccof[i - 1] / i;
			ccof[n - i]=ccof[i];
		}
		ccof[n - 1]=n;
		ccof[n]=1;
		return ccof1;
	}
	// Calculates the B coefficients for a highpass filter.
	std::shared_ptr<IntArray> IIRUtil::calculateBCoefficients_hp(uint16_t n){
		auto ccof1=calculateBCoefficients_lp(n);
		IntArray & ccof=*ccof1;
		for(u_index i=1;i <= n;i+=2){
			ccof[i]=-ccof[i];
		}
		return ccof1;
	}
	// Calculates the B coefficients for a bandpass filter.
	std::shared_ptr<IntArray> IIRUtil::calculateBCoefficients_bp(uint16_t n){
		auto tcof1=calculateBCoefficients_hp(n);
		IntArray & tcof=*tcof1;
		auto ccof1=std::make_shared<IntArray>(2 * n + 1);
		IntArray & ccof=*ccof1;
		for(u_index i=0;i < n;i++){
			ccof[2 * i]=tcof[i];
			ccof[2 * i + 1]=0;
		}
		ccof[2 * n]=tcof[n];
		return ccof1;
	}
	// Calculates the B coefficients for a bandstop filter.
	std::shared_ptr<DoubleArray> IIRUtil::calculateBCoefficients_bs(uint16_t n, u_freq f1f, u_freq f2f){
		double alpha=-2 * cos(2 * Math::PI * (f2f + f1f) / 2) / cos(2 * Math::PI * (f2f - f1f) / 2);
		auto ccof1=std::make_shared<DoubleArray>(2 * n + 1);
		DoubleArray & ccof=*ccof1;
		ccof[0]=1;
		ccof[1]=alpha;
		ccof[2]=1;
		for(u_index i=1;i < n;i++){
			ccof[2 * i + 2]+=ccof[2 * i];
			for(u_index j=2 * i;j > 1;j--){
				ccof[j + 1]+=alpha * ccof[j] + ccof[j - 1];
			}
			ccof[2]+=alpha * ccof[1] + (double)1;
			ccof[1]+=alpha;
		}
		return ccof1;
	}
	// Calculates the scaling factor for a lowpass or highpass filter.
	double IIRUtil::calculateScalingFactor_lp_hp(FilterPassType filterPassType, uint16_t n, u_freq fcf){
		double omega=2 * Math::PI * fcf;
		double fomega=sin(omega);
		double parg0=Math::PI / (2 * n);                       // zeroth pole angle
		int m=n / 2;
		double sf=1;                                          // scaling factor
		for(u_index k=0;k < n / 2;k++){
			sf*=1 + fomega * sin((2 * k + 1) * parg0);
		}
		double fomega2;
		switch(filterPassType){
			case LOWPASS:
			{
				fomega2=sin(omega / 2);
				if(n % 2 != 0){
					sf*=fomega2 + cos(omega / 2);
				}
				break;
			}
			case HIGHPASS:
			{
				fomega2=cos(omega / 2);
				if(n % 2 != 0){
					sf*=fomega2 + sin(omega / 2);
				}
				break;
			}
			default:
				throw Exception();
		}
		sf=pow(fomega2, n) / sf;
		return sf;
	}
	// Calculates the scaling factor for a bandpass or bandstop filter.
	double IIRUtil::calculateScalingFactor_bp_bs(FilterPassType filterPassType, uint16_t n, u_freq f1f, u_freq f2f){
		double tt=tan(2 * Math::PI * (f2f - f1f) / 2);    // tangent of theta
		double ctt_tt=(filterPassType == FilterPassType::BANDPASS)?1 / tt:tt; // contangent or tangent of theta
		double sfr=1;                                         // real part of the scaling factor
		double sfi=0;                                         // imaginary part of the scaling factor
		for(u_index k=0;k < n;k++){
			double parg=Math::PI * (2 * k + 1) / (2 * n);       // pole angle
			double sparg=ctt_tt + sin(parg);
			double cparg=cos(parg);
			double a=(sfr + sfi) * (sparg - cparg);
			double b=sfr * sparg;
			double c=-sfi * cparg;
			sfr=b - c;
			sfi=a - b - c;
		}
		return 1 / sfr;
	}
	//------------------------------------------------------------------------------
	// Multiplies a series of binomials together and returns the coefficients of
	// the resulting polynomial.
	//
	// The multiplication has the following form:
	//
	// (x + (*p)[0]) * (x + (*p)[1]) * ... * (x + (*p)[n-1])
	//
	// The (*p)[i] coefficients are assumed to be complex and are passed to the
	// function as an array of doubles of length 2n.
	//
	// The resulting polynomial has the following form:
	//
	// x^n + a[0]*x^n-1 + a[1]*x^n-2 + ... +a[n-2]*x + a[n-1]
	//
	// The a[i] coefficients can in general be complex but should in most
	// cases turn out to be real. The a[i] coefficients are returned by the
	// function as an array of doubles of length 2n.
	//
	// Function arguments:
	//  n  The number of binomials to multiply
	//  p  Array of doubles where p[2i] (i=0...n-1) is assumed to be the
	//     real part of the coefficient of the ith binomial and p[2i+1]
	//     is assumed to be the imaginary part. The overall size of the
	//     array is then 2n.
	std::shared_ptr<DoubleArray> IIRUtil::binomial_mult(uint16_t n, DoubleArray & p){
		auto aa=std::make_shared<DoubleArray>(2 * n);
		DoubleArray & a=*aa;
		for(u_index i=0;i < n;i++){
			for(u_index j=i;j > 0;j--){
				a[2 * j]+=p[2 * i] * a[2 * (j - 1)] - p[2 * i + 1] * a[2 * (j - 1) + 1];
				a[2 * j + 1]+=p[2 * i] * a[2 * (j - 1) + 1] + p[2 * i + 1] * a[2 * (j - 1)];
			}
			a[0]+=p[2 * i];
			a[1]+=p[2 * i + 1];
		}
		return aa;
	}
	// Multiplies a series of trinomials together and returns the coefficients
	// of the resulting polynomial.
	//
	// The multiplication has the following form:
	//
	// (x^2 + (*b)[0]x + (*c)[0]) * (x^2 + (*b)[1]x + (*c)[1]) * ... * (x^2 + (*b)[n-1]x + (*c)[n-1])
	//
	// The (*b)[i] and (*c)[i] coefficients are assumed to be complex and are passed
	// to the function as arrays of doubles of length 2n. The real part of the
	// coefficients are stored in the even numbered elements of the array and
	// the imaginary parts are stored in the odd numbered elements.
	//
	// The resulting polynomial has the following form:
	//
	// x^2n + a[0]*x^2n-1 + a[1]*x^2n-2 + ... +a[2n-2]*x + a[2n-1]
	//
	// The a[i] coefficients can in general be complex but should in most cases
	// turn out to be real. The a[i] coefficients are returned by the function as
	// an array of doubles of length 4n. The real and imaginary parts are stored,
	// respectively, in the even and odd elements of the array.
	//
	// Function arguments:
	//  n  The number of trinomials to multiply
	//  b  An array of doubles of length 2n.
	//  c  An array of doubles of length 2n.
	std::shared_ptr<DoubleArray> IIRUtil::trinomial_mult(uint16_t n, DoubleArray & b, DoubleArray & c){
		auto aa=std::make_shared<DoubleArray>(4 * n);
		DoubleArray & a=*aa;
		a[2]=c[0];
		a[3]=c[1];
		a[0]=b[0];
		a[1]=b[1];
		for(u_index i=1;i < n;i++){
			a[2 * (2 * i + 1)]+=c[2 * i] * a[2 * (2 * i - 1)] - c[2 * i + 1] * a[2 * (2 * i - 1) + 1];
			a[2 * (2 * i + 1) + 1]+=c[2 * i] * a[2 * (2 * i - 1) + 1] + c[2 * i + 1] * a[2 * (2 * i - 1)];
			for(u_index j=2 * i;j > 1;j--){
				a[2 * j]+=b[2 * i] * a[2 * (j - 1)] - b[2 * i + 1] * a[2 * (j - 1) + 1] + c[2 * i] * a[2 * (j - 2)] - c[2 * i + 1] * a[2 * (j - 2) + 1];
				a[2 * j + 1]+=b[2 * i] * a[2 * (j - 1) + 1] + b[2 * i + 1] * a[2 * (j - 1)] + c[2 * i] * a[2 * (j - 2) + 1] + c[2 * i + 1] * a[2 * (j - 2)];
			}
			a[2]+=b[2 * i] * a[0] - b[2 * i + 1] * a[1] + c[2 * i];
			a[3]+=b[2 * i] * a[1] + b[2 * i + 1] * a[0] + c[2 * i + 1];
			a[0]+=b[2 * i];
			a[1]+=b[2 * i + 1];
		}
		return aa;
	}
	double IIRUtil::groupDelay(IIR & iir, u_freq freq, u_sample_rate sampleRate){
		return groupDelay(iir.aCoeff, iir.bCoeff, freq, sampleRate);
	}
	double IIRUtil::groupDelay(BiquadIIR & iir, u_freq freq, u_sample_rate sampleRate){
		return groupDelay(iir.aCoeff, iir.bCoeff, freq, sampleRate);
	}
	double IIRUtil::groupDelay(DoubleArray & A, DoubleArray & B, u_freq freq, u_sample_rate sampleRate){
		int32_t i;
		double omega;
		double Br[2]{0, 0};
		double Be[2]{0, 0};
		double Ar[2]{0, 0};
		double Ae[2]{1, 0};
		double BrxAe[2]{0, 0};
		double BexAr[2]{0, 0};
		double AxB[2]{0, 0};
		double Num[2]{0, 0};
		double c[2]{0, 0};
		double magd2;
		omega=2.0f * Math::PI * freq / sampleRate;
		for(i=1;i < B.length;i++){
			Br[0]+=i * cos(i * 1.0 * omega) * B[i];
			Br[1]-=i * sin(i * 1.0 * omega) * B[i];
		}
		for(i=0;i < B.length;i++){
			Be[0]+=cos(i * 1.0 * omega) * B[i];
			Be[1]-=sin(i * 1.0 * omega) * B[i];
		}
		for(i=0;i < A.length;i++){
			Ar[0]+=(i + 1) * cos((i + 1.0) * omega) * A[i];
			Ar[1]-=(i + 1) * sin((i + 1.0) * omega) * A[i];
		}
		for(i=0;i < A.length;i++){
			Ae[0]+=cos((i + 1.0) * omega) * A[i];
			Ae[1]-=sin((i + 1.0) * omega) * A[i];
		}
		BrxAe[0]=Br[0] * Ae[0] - Br[1] * Ae[1];
		BrxAe[1]=Br[0] * Ae[1] + Br[1] * Ae[0];
		BexAr[0]=Be[0] * Ar[0] - Be[1] * Ar[1];
		BexAr[1]=Be[0] * Ar[1] + Be[1] * Ar[0];
		AxB[0]=Ae[0] * Be[0] - Ae[1] * Be[1];
		AxB[1]=Ae[0] * Be[1] + Ae[1] * Be[0];
		Num[0]=BrxAe[0] - BexAr[0];
		Num[1]=BrxAe[1] - BexAr[1];
		magd2=AxB[0] * AxB[0] + AxB[1] * AxB[1];
		c[0]=(Num[0] * AxB[0] + Num[1] * AxB[1]) / magd2;
		c[1]=(Num[1] * AxB[0] - Num[0] * AxB[1]) / magd2;
		return c[0];
	}
	std::shared_ptr<IIR> IIRUtil::newC1C3IIRFilter(u_freq freq, double c1, double c3){
		double g=1.0 - c1 / freq;
		double b=4.0 * c3 + freq;
		double a1=(-b + sqrt(b * b - 16.0 * c3 * c3)) / (4.0 * c3);
		DoubleArray aCoeff({static_cast<u_sample>(a1)});
		DoubleArray bCoeff({static_cast<u_sample>(g * (1 + a1))});
		return std::make_shared<IIR>(aCoeff, bCoeff);
	}
	void IIRUtil::C1C3IIRFilter(IIR & iir, u_freq freq, double c1, double c3){
		double g=1.0 - c1 / freq;
		double b=4.0 * c3 + freq;
		double a1=(-b + sqrt(b * b - 16.0 * c3 * c3)) / (4.0 * c3);
		iir.aCoeff[0]=a1;
		iir.bCoeff[0]=g * (1 + a1);
	}
	std::shared_ptr<DoubleArray> IIRUtil::cascade(DoubleArray & poly1, DoubleArray & poly2){
		auto result=std::make_shared<DoubleArray>(poly1.length + poly2.length - 1);
		DoubleArray & result1=*result;
		for(u_index i=0; i < poly1.length; ++i){
			for(u_index j=0; j < poly2.length; ++j){
				result1[i + j]+=poly1[i] * poly2[j];
			}
		}
		return result;
	}
	void IIRUtil::biquad(BiquadIIR & iir, u_freq f0, u_sample_rate sampleRate, double Q, FilterPassType type, double gain){
		iir.freq=f0;
		iir.q=Q;
		iir.type=type;
		iir.gain=gain;
		biquad(iir.aCoeff, iir.bCoeff, f0, sampleRate, Q, type, gain);
	}
	/**
	 *
	 * @param f0 频率
	 * @param sampleRate 采样率
	 * @param Q 品质因数
	 * @param type 类别
	 * {@link FilterPassType.NOTCH} 陷波，Q越大越陡，影响带宽越小
	 * {@link FilterPassType.BANDPASS} 带通，Q越大带宽越小，Q越小带宽越大
	 * {@link FilterPassType.LOWPASS} 低通，Q越大越陡越谐振，Q越小越平缓
	 * {@link FilterPassType.HIGHPASS} 高通，Q越大越陡越谐振，Q越小越平缓
	 * @return
	 */
	void IIRUtil::biquad(DoubleArray & aCoeff, DoubleArray & bCoeff, u_freq f0, u_sample_rate sampleRate, double Q, FilterPassType type, double gain){
		if(aCoeff.length != 2 || bCoeff.length != 3)throw Exception("Biquad coeff length error");
		double A=pow(10, gain / 40.0); // 将增益从dB转换为线性刻度
		double omega=2 * Math::PI * f0 / sampleRate;
		double sin_omega=sin(omega);
		double cos_omega=cos(omega);
		double alpha=sin_omega / (2 * Q);
		double beta=sqrt(A) / Q;
		switch(type){
			case LOWPASS:
			{
				double a0=1 + alpha;
				double a1=-2 * cos_omega;
				double a2=1 - alpha;
				double b0=(1 - cos_omega) / 2;
				double b1=1 - cos_omega;
				double b2=(1 - cos_omega) / 2;

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			case HIGHPASS:
			{
				double a0=1 + alpha;
				double a1=-2 * cos_omega;
				double a2=1 - alpha;
				double b0=(1 + cos_omega) / 2;
				double b1=-(1 + cos_omega);
				double b2=(1 + cos_omega) / 2;

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			case BANDPASS:
			{
				double a0=1 + alpha;
				double a1=-2 * cos_omega;
				double a2=1 - alpha;
				double b0=alpha;
				double b1=0;
				double b2=-alpha;

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			case BANDSTOP:
			case NOTCH:
			{
				double a0=1 + alpha;
				double a1=-2 * cos_omega;
				double a2=1 - alpha;
				double b0=1;
				double b1=-2 * cos_omega;
				double b2=1;

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			case LOWSHELF:
			{
				double a0=(A + 1) + (A - 1) * cos_omega + beta * sin_omega;
				double a1=-2 * ((A - 1) + (A + 1) * cos_omega);
				double a2=(A + 1) + (A - 1) * cos_omega - beta * sin_omega;
				double b0=A * ((A + 1) - (A - 1) * cos_omega + beta * sin_omega);
				double b1=2 * A * ((A - 1) - (A + 1) * cos_omega);
				double b2=A * ((A + 1) - (A - 1) * cos_omega - beta * sin_omega);

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			case HIGHSHELF:
			{
				double a0=(A + 1) - (A - 1) * cos_omega + beta * sin_omega;
				double a1=2 * ((A - 1) - (A + 1) * cos_omega);
				double a2=(A + 1) - (A - 1) * cos_omega - beta * sin_omega;
				double b0=A * ((A + 1) + (A - 1) * cos_omega + beta * sin_omega);
				double b1=-2 * A * ((A - 1) + (A + 1) * cos_omega);
				double b2=A * ((A + 1) + (A - 1) * cos_omega - beta * sin_omega);

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			case BELL:
			{
				double a0=1 + alpha / A;
				double a1=-2 * cos_omega;
				double a2=1 - alpha / A;
				double b0=1 + alpha * A;
				double b1=-2 * cos_omega;
				double b2=1 - alpha * A;

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			case ALLPASS:
			{
				double a0=1 + alpha;
				double a1=-2 * cos_omega;
				double a2=1 - alpha;
				double b0=1 - alpha;
				double b1=-2 * cos_omega;
				double b2=1 + alpha;

				bCoeff[0]=b0 / a0;
				bCoeff[1]=b1 / a0;
				bCoeff[2]=b2 / a0;
				aCoeff[0]=a1 / a0;
				aCoeff[1]=a2 / a0;
			}
			break;
			default:
				throw Exception("Unsupported type");
		}
	}
	std::shared_ptr<BiquadIIR> IIRUtil::newBiquadIIRFilter(u_freq f0, u_sample_rate sampleRate, double Q, FilterPassType type){
		return newBiquadIIRFilter(f0, sampleRate, Q, 0, type);
	}
	std::shared_ptr<BiquadIIR> IIRUtil::newBiquadIIRFilter(u_freq f0, u_sample_rate sampleRate, double Q, double gain, FilterPassType type){
		std::shared_ptr<BiquadIIR> iir=std::make_shared<BiquadIIR>();
		biquad(*iir, f0, sampleRate, Q, type, gain);
		return iir;
	}

	// 基于带宽、频率和滤波器阶数计算Thiran滤波器的延迟参数
	// 定义Thiran延迟计算的基准系数结构体
	struct ThiranCoeffBenchmark{
		int32_t order;       // 滤波器阶数（偶数）
		double constant1;    // 校正因子系数1
		double constant2;    // 校正因子系数2
		double factor1;      // 延迟因子系数1（二次项）
		double factor2;      // 延迟因子系数2（一次项）
		double factor3;      // 延迟因子系数3（常数项）
	};

	// 基准系数表：支持2/4/6/8阶（工程常用偶数阶）
	const std::vector<ThiranCoeffBenchmark> THIRAN_COEFF_BENCHMARKS={
		{1, 0.071089, 2.1074, -0.0026580, -0.014811f, -2.9018},
		{2, 0.070561, 2.0821, -0.0010123, -0.010195, -2.8994},
		{4, 0.069618, 2.0427, -0.00050469, -0.0064264, -2.8743},
		{6, 0.067035, 1.9312, 0.0028917, 0.0082637, -2.8281},
		{8, 0.064782, 1.8355, 0.0057632, 0.021874, -2.7892}
	};

	// 辅助函数：根据目标阶数，通过线性插值获取动态系数
	ThiranCoeffBenchmark getDynamicThiranCoeffs(int32_t targetOrder){
		// 1. 阶数预处理：Thiran全通仅支持偶数阶，且限制在2~8阶（避免计算发散）
		// - 奇数阶：向上取整为最近偶数（如5→6，7→8）
		// - 超出范围：低于2阶→2阶，高于8阶→8阶
		int32_t processedOrder=Util::clamp(targetOrder, 1, 8);

		// 2. 查找相邻的基准阶数（lowOrder ≤ processedOrder ≤ highOrder）
		ThiranCoeffBenchmark lowCoeff, highCoeff;
		for(u_index i=0; i < THIRAN_COEFF_BENCHMARKS.size() - 1; ++i){
			lowCoeff=THIRAN_COEFF_BENCHMARKS[i];
			highCoeff=THIRAN_COEFF_BENCHMARKS[i + 1];
			if(processedOrder >= lowCoeff.order && processedOrder <= highCoeff.order){
				break;
			}
		}

		// 3. 线性插值计算动态系数（若目标阶数等于基准阶数，插值系数为0或1，直接返回基准值）
		double t=0.0; // 插值权重（0→完全用lowCoeff，1→完全用highCoeff）
		if(highCoeff.order != lowCoeff.order){
			t=static_cast<double>(processedOrder - lowCoeff.order) / (highCoeff.order - lowCoeff.order);
		}

		ThiranCoeffBenchmark dynamicCoeff;
		dynamicCoeff.order=processedOrder;
		// 对每个系数进行线性插值：coeff = low * (1-t) + high * t
		dynamicCoeff.constant1=lowCoeff.constant1 * (1 - t) + highCoeff.constant1 * t;
		dynamicCoeff.constant2=lowCoeff.constant2 * (1 - t) + highCoeff.constant2 * t;
		dynamicCoeff.factor1=lowCoeff.factor1 * (1 - t) + highCoeff.factor1 * t;
		dynamicCoeff.factor2=lowCoeff.factor2 * (1 - t) + highCoeff.factor2 * t;
		dynamicCoeff.factor3=lowCoeff.factor3 * (1 - t) + highCoeff.factor3 * t;

		return dynamicCoeff;
	}
	double IIRUtil::calculateThiranDelay(double dispersionFactor, double frequency, int32_t filterOrder){
		// 1. 动态获取当前阶数对应的系数（替代原代码的固定if-else）
		ThiranCoeffBenchmark coeff=getDynamicThiranCoeffs(filterOrder);
		double constant1=coeff.constant1;
		double constant2=coeff.constant2;
		double factor1=coeff.factor1;
		double factor2=coeff.factor2;
		double factor3=coeff.factor3;

		// 2. 原代码的延迟计算逻辑（保持不变，确保业务逻辑连续）
		// 计算对数带宽（dispersionFactor：色散系数，描述频率带宽特性）
		double dispersionFactorLog=log(dispersionFactor);

		// 计算延迟因子（与色散系数相关，控制延迟随带宽的变化）
		double delayFactor=exp(
			factor1 * dispersionFactorLog * dispersionFactorLog +  // 二次项：带宽对延迟的非线性影响
			factor2 * dispersionFactorLog +                        // 一次项：带宽对延迟的线性影响
			factor3                                                // 常数项：基础延迟偏移
		);

		// 计算校正因子（补偿阶数带来的延迟偏差）
		double correctionFactor=exp(
			constant1 * dispersionFactorLog +  // 带宽对校正的影响
			constant2                          // 基础校正偏移
		);

		double halfStepRatio=pow(2.0, 1.0 / 12.0);
		double noteNumber=log(frequency * halfStepRatio / 27.5f) / log(halfStepRatio);

		// 3. 计算最终延迟（结合校正因子、音符编号、延迟因子）
		return exp(correctionFactor - noteNumber * delayFactor);
	}

	// 设计Thiran全通滤波器，计算滤波器系数
	void IIRUtil::designThiranFilter(BiquadIIR & filter, double targetDelay){
		designThiranFilter(filter.aCoeff, filter.bCoeff, targetDelay, 2);
	}
	void IIRUtil::designThiranFilter(DoubleArray & aCoeff, DoubleArray & bCoeff, double targetDelay, int filterStages){
		int stage, numeratorTerm, denominatorTerm;

		// 确保目标延迟不小于滤波器阶数减1
		if(targetDelay < filterStages - 1)
			targetDelay=filterStages - 1.0f;

		// 设置反馈系数的最后一项为1（a2 = 1）
		bCoeff[filterStages]=1;

		// 计算滤波器系数
		for(stage=1; stage <= filterStages; stage++){
			// 计算组合数 C(filterStages, stage)
			double coefficientValue=Util::NChoose(filterStages, stage);

			// 奇数阶系数取负值（交替符号）
			if(stage % 2 == 1)
				coefficientValue=-coefficientValue;

			// 计算分子部分
			for(numeratorTerm=0; numeratorTerm < stage; numeratorTerm++){
				coefficientValue*=(targetDelay - ((double)filterStages - numeratorTerm));
			}

			// 计算分母部分
			for(denominatorTerm=0; denominatorTerm < stage; denominatorTerm++){
				coefficientValue/=(targetDelay - ((double)-stage + denominatorTerm));
			}

			// 设置前馈系数（b[filterStages - stage]）
			aCoeff[stage - 1]=coefficientValue;

			// 设置反馈系数（a[filterStages - stage]）
			bCoeff[filterStages - stage]=coefficientValue;
		}
	}
	u_freq IIRUtil::limitFreq(u_sample_rate sampleRate, u_freq f){
		return Util::clamp(f, 10.0, static_cast<u_freq>(sampleRate) * 0.99 / 2.0);
	}
}